#include "cvxImage.h"
#include <iostream>
#include <time.h>
#include <vector>
#include <algorithm>
#include "IntrinsicLS.h"
#include "IntrinsicSolver.h"
#include "testIntrinsicImage.h"

using namespace std;
using namespace cv;
using cvx::Rgb16UImage;
using cvx::Rgb32FImage;
using cvx::Bw64FImage;
using cvx::Argb8UImage;
using cvx::Bw32FImage;
using cvx::Bw8UImage;
using cvx::Rgb8UImage;

#if 1
//test local shading model
int main()
{
//	TestIntrinsicImage::testEdgeMap();
//	TestIntrinsicImage::testLocalshadingWidthEdgeMap();
//	TestIntrinsicImage::testLeastSquare();
//	TestIntrinsicImage::testExpanding();
//	TestIntrinsicImage::testSCS_matrix();
//	TestIntrinsicImage::testBVLS();
//	TestIntrinsicImage::testCSC_extractCols();
//	TestIntrinsicImage::testIterateExpand();
//	TestIntrinsicImage::testQuadtree();
//	TestIntrinsicImage::test2X2Window();
//	TestIntrinsicImage::testCvxRandomQuadTree();
//	TestIntrinsicImage::testCvxSimColorQuadTree();
//	TestIntrinsicImage::testRecoverA();
//	TestIntrinsicImage::testMinimalShading();
//	TestIntrinsicImage::testSegmentation();
//	TestIntrinsicImage::testDisparity();
//	TestIntrinsicImage::testComputeDisparity();
//	TestIntrinsicImage::testQuadtreeSolver();
//	TestIntrinsicImage::testReverseTree();
//	TestIntrinsicImage::testEMTree();
	TestIntrinsicImage::testEMTreeSolver();
//	TestIntrinsicImage::testAVectorInQuadTree();
//	TestIntrinsicImage::testSimilarReflectane();
//	TestIntrinsicImage::testCvxQuadStroke();
//	TestIntrinsicImage::testGeneralQuadtreeImage();
//	TestIntrinsicImage::testCholmod();
//	TestIntrinsicImage::testMultipleScaleSample();
//	TestIntrinsicImage::testCvxLocalSolver();
//	TestIntrinsicImage::testQuadInterpolate();
//	TestIntrinsicImage::testLocalRefinement();


	cvWaitKey(0);
}
#endif

#if 0
//test average assumption
int main()
{
	Mat orgImg = imread("average_test\\synthetic.tif", 1);
	Mat shaImg_true= imread("average_test\\synthetic_shading.png", 0);
	assert(!orgImg.empty());
	assert(!shaImg_true.empty());

	//124 76
	cv::Rect rect = Rect(400, 180, 124*1.5, 76*1.5);

	Mat subImg = orgImg(rect);
	Mat subShaImg = shaImg_true(rect);

	imshow("subImg", subImg);
	imshow("ground truth", subShaImg);
	
	int w = rect.width;
	int h = rect.height;
	//average color
	int num = w * h;

	double r_avg = 0.0;
	double g_avg = 0.0;
	double b_avg = 0.0;

	Rgb8UImage sun_img(&subImg);
	for (int y = 0; y<h; ++y)
	{
		for (int x = 0; x<w; ++x)
		{
			r_avg += sun_img[y][x].r;
			g_avg += sun_img[y][x].g;
			b_avg += sun_img[y][x].b;
		}
	}

	r_avg /= num;
	g_avg /= num;
	b_avg /= num;

	double max_v = std::max(r_avg, g_avg);
	max_v = std::max(max_v, b_avg);
	double s = max_v/255.0;
	cout<<"minimal s = "<<s<<endl;
	//set average color shading value
	Mat refMat = subImg.clone();
	Mat shaMat = cv::Mat(h, w, CV_8U);

	for (double is = s; is<1.0; is += 0.02)
	{
		double r = r_avg/is;
		double g = g_avg/is;
		double b = b_avg/is;

		cout<<"r = "<<r<<endl;
		cout<<"g = "<<g<<endl;
		cout<<"b = "<<b<<endl;

		refMat = Scalar(b, g, r);
		imshow("ref", refMat);
	//	cvWaitKey(0);
		cout<<"s = "<<is<<endl;
		cout<<endl;

		Bw8UImage s_map(&shaMat);
		for (int y = 0; y<h; ++y)
		{
			for (int x = 0;x <w; ++x)
			{
				double sVal = sun_img[y][x].r/is;
				if (sVal > 255)
				{
		//			cout<<"sVal = "<<sVal<<endl;
				}
				s_map[y][x] = sVal;
			}
		}
		imshow("shading map", shaMat);
		cvWaitKey(0);



	}

	cvWaitKey(0);	

	return 0;
}
#endif

#if 0
//test synthetic image 
int main()
{
 	Mat synImage = imread("synthetic\\synthetic.tif", 1);
 	Mat synRefImage = imread("synthetic\\synthetic_albedo.png", 1);  
 	Mat synShaImage = imread("synthetic\\synthetic_shading_truth.tif", 0);

//  	Mat synImage = imread("flower\\pink_flower.tif", 1);
//  	Mat synRefImage = imread("flower\\albedo.png", 1);  
//  	Mat synShaImage = imread("flower\\shading.png", 0);

	assert(!synImage.empty());
	assert(!synRefImage.empty());
	assert(!synRefImage.empty());

//	synShaImage.convertTo(synShaImage, CV_32F, 1.0/255.0);
	int w = synImage.cols;
	int h = synImage.rows;

	cv::Size hSize = cv::Size(w/4, h/4);


	cv::resize(synImage, synImage, hSize, 0, 0, CV_INTER_NN);
	cv::resize(synRefImage, synRefImage, hSize, 0, 0, CV_INTER_NN);
	cv::resize(synShaImage, synShaImage, hSize, 0, 0, CV_INTER_NN);
	
	imshow("syn image before", synImage);

	w = synImage.cols;
	h = synImage.rows;


//	imshow("syn image after", synImage);
//	imshow("reflectance image", synRefImage);
	imshow("shading", synShaImage);

	test_4DlinearModel(synImage, synShaImage);
	test_4DlinearModelB(synImage, synShaImage);
	cvWaitKey(0);

	return 1;
}
#endif

#if 0
int main()
{
//  	string orgFile = string("flower\\pink_flower.tif");
//  	string absoluteShading = string("flower\\scribbleAbsolute.png");
//  	string simAlbedo	   = string("flower\\scribbleSimAlbedo.png");
//  	string simShading      = string("flower\\scribbleSimShading.png");

 	string orgFile = string("synthetic\\synthetic.tif");
 	string absoluteShading = string("synthetic\\scribbleAbsolute.png");
 	string simAlbedo	   = string("synthetic\\scribbleSimAlbedo.png");
 	string simShading      = string("synthetic\\scribbleSimShading.png");


	Mat orgImg = imread(orgFile, 1);
	assert(!orgImg.empty());	

	//	buildLMatrix(org);
	int w = orgImg.cols;
	int h = orgImg.rows;
	cv::Size hSize = cv::Size(w/8, h/8);	

	Mat absSha = imread(absoluteShading, 0);
	Mat simRef = imread(simAlbedo, 0);
	Mat simSha = imread(simShading, 0);

	assert(!absSha.empty());
	assert(!simRef.empty());
	assert(!simSha.empty());

	cout<<"channel = "<<absSha.channels()<<endl;
	cout<<"channel = "<<simRef.channels()<<endl;
	cout<<"channel = "<<simSha.channels()<<endl;

	cv::resize(orgImg, orgImg, hSize, 0, 0, INTER_LINEAR);
	cv::resize(absSha, absSha, hSize, 0, 0, INTER_NEAREST);
	cv::resize(simRef, simRef, hSize, 0, 0, INTER_NEAREST);
	cv::resize(simSha, simSha, hSize, 0, 0, INTER_NEAREST);

	imshow("abs shading", absSha);
	imshow("sim reflection", simRef);
	imshow("sim shading", simSha);
	imshow("flower", orgImg);

	//different reflectance pixel value
	vector<int> simRefPixVal;	
	vector<vector<int>> simRefPixPos;

	w = orgImg.cols;
	h = orgImg.rows;

	Bw8UImage sim_ref(&simRef);
	for (int y = 0; y<hSize.height; ++y)
	{
		for (int x = 0; x<hSize.width; ++x)
		{
			int val = sim_ref[y][x];
			if (val != 0)
			{
				vector<int>::iterator ite = std::find(simRefPixVal.begin(), simRefPixVal.end(), val);
				if (ite == simRefPixVal.end())
				{
					simRefPixVal.push_back(val);
					vector<int> pos;
					pos.push_back(y * w + x);
					simRefPixPos.push_back(pos);
				}
				else
				{
					int dif = ite - simRefPixVal.begin();
					simRefPixPos[dif].push_back(y*w+x);
				}			
			}		
		}
	}
	
	vector<int> simShaPixVal;
	vector<vector<int>> simShaPixPos;

	Bw8UImage sim_sha(&simSha);
	for (int y = 0; y<hSize.height; ++y)
	{
		for (int x = 0; x<hSize.width; ++x)
		{
			int val = sim_sha[y][x];
			if (val != 0)
			{
				vector<int>::iterator ite = std::find(simShaPixVal.begin(), simShaPixVal.end(), val);
				if (ite == simShaPixVal.end())
				{
					simShaPixVal.push_back(val);
					vector<int> pos;
					pos.push_back(y * w + x);
					simShaPixPos.push_back(pos);
				}
				else
				{
					int dif = ite - simShaPixVal.begin();
					simShaPixPos[dif].push_back(y*w+x);
				}			
			}		
		}
	}

	for (int i = 0;i<simShaPixPos.size(); ++i)
	{
		cout<<"size = "<<simShaPixPos[i].size()<<endl;
	}
	//	return 0;

	//test Lab color space
	Mat labImage;
	cv::cvtColor(orgImg, labImage, COLOR_BGR2Lab);

	imshow("lab image", labImage);
	vector<Mat> mv;
	cv::split(labImage, mv);
	if (mv.size() != 0)
	{
		imshow("L channel", mv[0]);
		//	imshow("a channel", mv[1]);
		//	imshow("b channel", mv[2]);
	}

	solve_intrinsic_ls_area(orgImg, absSha, simRefPixPos, simShaPixPos);

	cvWaitKey(0);	

	return 1;
}
#endif


#if 0
int main()
{
	//computer L matrix
// 	string orgFile = string("flower\\pink_flower.tif");
// 	string absoluteShading = string("flower\\scribbleAbsolute.png");
// 	string simAlbedo	   = string("flower\\scribbleSimAlbedo.png");
// 	string simShading      = string("flower\\scribbleSimShading.png");

	string orgFile = string("synthetic\\synthetic.tif");
	string absoluteShading = string("synthetic\\scribbleAbsolute.png");
	string simAlbedo	   = string("synthetic\\scribbleSimAlbedo.png");
	string simShading      = string("synthetic\\scribbleSimShading.png");

	Mat orgImg = imread(orgFile, 1);
	assert(!orgImg.empty());	

	//	buildLMatrix(org);
	int w = orgImg.cols;
	int h = orgImg.rows;
	cv::Size hSize = cv::Size(w/4, h/4);


	

	Mat absSha = imread(absoluteShading, 0);
	Mat simRef = imread(simAlbedo, 0);
	Mat simSha = imread(simShading, 0);

	assert(!absSha.empty());
	assert(!simRef.empty());
	assert(!simSha.empty());

	cout<<"channel = "<<absSha.channels()<<endl;
	cout<<"channel = "<<simRef.channels()<<endl;
	cout<<"channel = "<<simSha.channels()<<endl;

	cv::resize(orgImg, orgImg, hSize, 0, 0, INTER_LINEAR);
	cv::resize(absSha, absSha, hSize, 0, 0, INTER_NEAREST);
	cv::resize(simRef, simRef, hSize, 0, 0, INTER_NEAREST);
	cv::resize(simSha, simSha, hSize, 0, 0, INTER_NEAREST);

	imshow("abs shading", absSha);
	imshow("sim reflection", simRef);
	imshow("sim shading", simSha);
	imshow("flower", orgImg);
	
	vector<int> simRefPixNum(256, 0);
	vector<int> simShaPixNum(256, 0);

	Bw8UImage sim_ref_pix_num(&simRef);
	Bw8UImage sim_sha_pix_num(&simSha);
	for (int y = 0; y<hSize.height; ++y)
	{
		for (int x = 0; x<hSize.width; ++x)
		{
			if (sim_ref_pix_num[y][x] != 0)
			{
				simRefPixNum[sim_ref_pix_num[y][x]]++;
			}

			if (sim_sha_pix_num[y][x] != 0)
			{
				simShaPixNum[sim_sha_pix_num[y][x]]++;
			}
		}
	}
	
	for (int i = 0; i<simShaPixNum.size(); ++i)
	{
		if (simShaPixNum[i] != 0)
		{
			cout<<simShaPixNum[i]<<endl;
		}
	}


//	return 0;

	//test Lab color space
	Mat labImage;
	cv::cvtColor(orgImg, labImage, COLOR_BGR2Lab);

	imshow("lab image", labImage);
	vector<Mat> mv;
	cv::split(labImage, mv);
	if (mv.size() != 0)
	{
		imshow("L channel", mv[0]);
	//	imshow("a channel", mv[1]);
	//	imshow("b channel", mv[2]);
	}

	solve_intrinsic_ls(orgImg, absSha, simSha, simRef, simRefPixNum, simShaPixNum);

	cvWaitKey(0);	

	return 1;
}

#endif

//----- LMatrix initialization -----//

const int WSIZE   = 3;    //size of the local windows
const int WSIZE2  = WSIZE*WSIZE;   //WSIZE2 = WSIZE*WSIZE, number of pixels in each window ('n' in the paper)
const int MSIZE   = WSIZE2+3;   //WSIZE2+3, number of rows in the M matrix (see equation 8)
const int WSIZE_2 = WSIZE/2;  //WSIZE_2 = WSIZE/2
const int SIZE_N  = 2*2*WSIZE_2+1;   //2*2*WSIZE_2+1, size of the total neighborhood affected by a pixel
const int SIZE_N2 = SIZE_N*SIZE_N; //SIZE_N*SIZE_N, number of non-zero element per-pixel in the L matrix 

//double ** LMatrix;


void buildLMatrix(const Mat &imgIn)
{
	assert(!imgIn.empty());
	assert(imgIn.depth() == CV_32F);
	assert(imgIn.channels() == 3);
	double epsilon = 0.0000001;
	const int w = imgIn.cols;
	const int h = imgIn.rows;
	double sqrt_epsilon = sqrt(epsilon);

	double M[MSIZE][3]; //M matrix (see equation 8)
	M[MSIZE-3][0] = sqrt_epsilon; M[MSIZE-3][1] = 0; M[MSIZE-3][2] = 0;
	M[MSIZE-2][0] = 0; M[MSIZE-2][1] = sqrt_epsilon; M[MSIZE-2][2] = 0;
	M[MSIZE-1][0] = 0; M[MSIZE-1][1] = 0; M[MSIZE-1][2] = sqrt_epsilon;

	int i,j,ik,jk;
	int x,y;
	int xC,yC,xiC,yiC,xjC,yjC;

	//Init the L matrix. L is a sparse matrix that only contains SIZE_N2 non-zero elements per row.
	//We store the L matrix in an array with one row per image pixel and one column per non-zero value. 
	//Each array cell accumulates values according to equation 11.
	// w * h * 25
	float **FLMatrix = new float* [w*h];
	for(i=0; i<w*h; i++){
		FLMatrix[i] = new float[SIZE_N2];
		for(j=0; j<SIZE_N2; j++){
			FLMatrix[i][j] = 0;
		}
	}

	Rgb32FImage in_img(&imgIn);
	//Compute the L matrix
	for( y = WSIZE_2; y < h-WSIZE_2; y++) {//for each pixel...
		for( x = WSIZE_2; x < w-WSIZE_2; x++)
		{
			//fill the M matrix with the image values in the current window
			for (int m=-WSIZE_2; m<=WSIZE_2; m++) {
				yC = y+m;
				for (int n=-WSIZE_2; n<=WSIZE_2; n++) {
					xC = x+n;
				//	M[(m+WSIZE_2)*WSIZE + n+WSIZE_2][0] = imgIn.getR(xC,yC);
				//	M[(m+WSIZE_2)*WSIZE + n+WSIZE_2][1] = imgIn.getG(xC,yC);
				//	M[(m+WSIZE_2)*WSIZE + n+WSIZE_2][2] = imgIn.getB(xC,yC);

					M[(m+WSIZE_2)*WSIZE + n+WSIZE_2][0] = in_img[yC][xC].r;
					M[(m+WSIZE_2)*WSIZE + n+WSIZE_2][1] = in_img[yC][xC].g;
					M[(m+WSIZE_2)*WSIZE + n+WSIZE_2][2] = in_img[yC][xC].b;
				}
			}
			


			//Compute the N matrix (equation 9 and 10)
			/*
			Matrix<MSIZE,3,double> Mi(M);
			Matrix<3,MSIZE,double> Mit(Mi.transpose());
			Square_matrix<3,double> MitMi(Mit*Mi);
			MitMi_inv = inverse_matrix(MitMi);
			Square_matrix<MSIZE,double> Ni(Mi*MitMi_inv*Mit);
			Ni = Ni.identity() - Ni;
			Square_matrix<MSIZE,double> Nit(Ni.transpose());
			Square_matrix<MSIZE,double> NitNi(Nit*Ni);
			*/
			Mat Mi  = Mat(MSIZE,3,CV_64F, M);
			Mat Mit = Mi.t();
			Mat MitMi = Mit * Mi;
			Mat MitMi_inv = MitMi.inv();
			Mat Ni = Mi * MitMi_inv * Mit;
			Ni = Mat::eye(Ni.rows, Ni.cols, CV_64F) - Ni;
			Mat Nit = Ni.t();
			Mat NitNi = Nit * Ni;


			//accumulate the values in the L matrix, for each couple of pixels (i,j) covered by the window
			//--- i
			Bw64FImage NitNi_img(&NitNi);
			for (int mi=-WSIZE_2; mi<=WSIZE_2; mi++) {//(mi,ni) = coordinate of pixel i in the local window
				yiC = y+mi; //(xiC,yiC) = coordinate of pixel i in the image
				for (int ni=-WSIZE_2; ni<=WSIZE_2; ni++) {
					xiC = x+ni;
					i = yiC*w+xiC; //row number in the matrix
					ik = (mi+WSIZE_2)*WSIZE + ni+WSIZE_2;
					//--- j
					for (int mj=-WSIZE_2; mj<=WSIZE_2; mj++) {
						yjC = y+mj;
						for (int nj=-WSIZE_2; nj<=WSIZE_2; nj++) {
							xjC = x+nj;
							jk = (mj+WSIZE_2)*WSIZE + nj+WSIZE_2;
							//accumulate values (equation 11)
							FLMatrix[i][(yiC-yjC+2*WSIZE_2)*SIZE_N+(xiC-xjC+2*WSIZE_2)] += NitNi_img[ik][jk];
							//pixel j is at coordinate (xiC-xjC,yiC-yjC) in the SIZE_N*SIZE_N neighborhood affected by pixel i (centered at pixel i)
						}
					}
				}
			}		
		}
		cout<<"y = " <<y<<endl;
	}

	//save FLMatrix
	float *tData = new float[w*h*SIZE_N2];
	assert(tData);
	for(i=0; i<w*h; i++){	
		memcpy((char *)tData + i * sizeof(tData[0])*SIZE_N2, &FLMatrix[i][0], sizeof(tData[0])*SIZE_N2);
	}
	cout<<"data copy complete."<<endl;

 	FileStorage fs("LMatrix.xml", FileStorage::WRITE);
 	Mat LMat = Mat(w*h, SIZE_N2, CV_32F, tData);
 	fs<<"LMatrix"<<LMat;
 	fs.release();
	delete []tData;
	cout<<"save L matrix to LMatrix.xml"<<endl;
}

//----- Gauss-Seidel iterations -----//

/*
imgScribbleAbsolute: image that stores the fixed-illumination scribbles (pixels store illumination values, alpha = true if the pixel is scribbled)
imgScribbleSimAlbedo: image that stores the constant-reflectance scribbles (pixels store scribbles ids)
imgScribbleSimShading: image that stores the constant-illumination scribbles (pixels store scribbles ids)
maxAlbedoNum: number of constant-reflectance scribble ids
maxShadingNum: number of constant-illumination scribble ids
albedoNumColors: constant-reflectance scribble ids
shadingNumColors: constant-illumination scribble ids
*/

#define EPSILON_PIXEL_VAL 0.001 //accuracy threshold

void solveA(const Mat & imgIn,
			const Mat & LMatrix,
			const Mat & imgScribbleAbsolute, 
		    const Mat & imgScribbleSimAlbedo,
		    const Mat & imgScribbleSimShading,
			const vector<Vec3b> &albedoNumColors, 
			const vector<Vec3b> &shadingNumColors)
{
	int w = imgIn.cols;
	int h = imgIn.rows;
	Mat result(h, w, CV_32FC3, Scalar(0.5, 0.5, 0.5, 0.5));
	double rRes, gRes, bRes;//temporary result values
//	double denom;
	double lambdaCstr = 0.5;//weight of the user constraints

	//variables used to compute user constraints
	double rSumI, gSumI, bSumI;//sum of images values
	double rSumS, gSumS, bSumS;//sum of illumination values
	double rAvrg, gAvrg, bAvrg;//average illumination
	Vec3f imgVal, resVal;
	
	vector<Vec3f> albedoAvgColors(albedoNumColors.size());   //average reflectance for each constant-reflectance scribble
	vector<Vec3f> shadingAvgColors(shadingNumColors.size()); //average illumination for each constant-illumination scribble

	//Gauss-Seidel iterations.
	//for simplicity, we apply here a fixed number of iterations

	Argb8UImage sim_alb(&imgScribbleSimAlbedo);
	Argb8UImage sim_sha(&imgScribbleSimShading);
	Argb8UImage sha_abs(&imgScribbleAbsolute);
	Rgb32FImage img_in(&imgIn);
	Rgb32FImage rst(&result);
	Bw32FImage  l_mat(&LMatrix);

	int nbIter = 100;
	for (int kk=0; kk<nbIter; kk++)
	{
		cout<<"kk = "<<kk<<endl;
		//-------------------------------//
		//--- User constraints values ---//
		//-------------------------------//

		//In order to apply the user constraints, we first need to compute the average value of constant reflectance 
		// or illumination pixels

		//-- Constant-reflectance
		for (int c=0; c<albedoNumColors.size(); c++)
		{
			//for each constant-reflectance scribble id
			//illumination estimation for pixel p = I(p)*sum(S)/sum(I), where S stands for illumination values of constant pixels and I for input value of constant pixels
			rSumI = gSumI = bSumI = 0;
			rSumS = gSumS = bSumS = 0;
			int nbP = 0;
			for (int y = 0; y < h; y++) {//for each pixel of the scribbles image
				for (int x = 0; x < w; x++) {
					if(sim_alb[y][x].r == albedoNumColors[c][0]){//pixel scribbled with the current constraint
						//get the image values for this pixel
						rSumI += img_in[y][x].r;						
						gSumI += img_in[y][x].g;
						bSumI += img_in[y][x].b;

						rSumS += rst[y][x].r;
						gSumS += rst[y][x].g;
						bSumS += rst[y][x].b;						
						nbP++;
					}
				}
			}
			if(nbP>0){
				//if there was some scribbled pixels
				// R = I/s
				albedoAvgColors[c] = Vec3f(rSumS/rSumI,gSumS/gSumI,bSumS/bSumI);
			}
		}

		//-- Constant-illumination
		for (int c=0; c<shadingNumColors.size(); c++)
		{//for each constant-illumination scribble id
			//illumination estimation = average illumination
			rAvrg = gAvrg = bAvrg = 0;
			int nbP = 0;
			for (int y = 0; y < h; y++) 
			{//for each pixel of the scribbles image
				for (int x = 0; x < w; x++) {
					if(sim_sha[y][x].r == shadingNumColors[c][0])
					{
						//pixel scribbled with the current constraint
						rAvrg += rst[y][x].r;
						gAvrg += rst[y][x].g;
						bAvrg += rst[y][x].b;
						nbP++;
					}
				}
			}
			if(nbP>0){//if there was some scribbled pixels
				rAvrg/=nbP;
				gAvrg/=nbP;
				bAvrg/=nbP;
				shadingAvgColors[c] = Vec3f(rAvrg, gAvrg, bAvrg);
			}
		}
	
	
	
	cout<<"albedo color "<<endl;
	for (int i = 0; i<albedoAvgColors.size(); ++i)
	{
		cout<<albedoAvgColors[i][0]<<" "<<albedoAvgColors[i][1]<<" "<<albedoAvgColors[i][2]<<endl;
	}
	cout<<endl;

	cout<<"shading color "<<endl;
	for (int i = 0; i<shadingNumColors.size(); ++i)
	{
		cout<<shadingAvgColors[i][0]<<" "<<shadingAvgColors[i][1]<<" "<<shadingAvgColors[i][2]<<endl;
	}
	cout<<endl;
	
	
		//------------------------------------------------//
		//--- Gauss-Seidel iteration with the L matrix ---//
		//------------------------------------------------//

		for (int y = 0; y < h; y++) 
		{
			for (int x = 0; x < w; x++) 
			{
				int idx = y*w+x; //row number for the current pixel
				//is that a fixed-illumination pixel?
			//	imgVal = scribbleAbsolute.pixel(x,y);
				if(sha_abs[y][x].a != 0)
				{//fixed-illumination pixel: impose the constraint
					//@todo
				//	cout<<"fixed illumination = "<<sha_abs[y][x].r<<" "<<sha_abs[y][x].g<<" "<<sha_abs[y][x].b<<endl;
					rRes = sha_abs[y][x].r / 255.0;
					gRes = sha_abs[y][x].g / 255.0;
					bRes = sha_abs[y][x].b / 255.0;
				}
				else
				{  
				//non scribbled pixel
					//-- Gauss-Seidel update
					rRes = gRes = bRes = 0;
					double denom = 0.0;
					for (int k=0; k < SIZE_N2; k++)
					{
						//for each value on the matrix row corresponding to the current pixel
						if(l_mat[idx][k]!=0)
						{
							//coordinate of the corresponding pixel in the image
							int yjC = -k/SIZE_N+2*WSIZE_2+y;
							int xjC = -k+(y-yjC+2*WSIZE_2)*SIZE_N+2*WSIZE_2+x;

							if(x==xjC && y==yjC)
							{//diagonal
								denom = l_mat[idx][k];
							}
							else
							{
							//	resVal = result.pixel(xjC,yjC);
								//relaxation
								rRes -= l_mat[idx][k] * rst[yjC][xjC].r;
								gRes -= l_mat[idx][k] * rst[yjC][xjC].g;
								bRes -= l_mat[idx][k] * rst[yjC][xjC].b;
							}
						}
					}

					if (denom != 0)
					{
						rRes /= denom;
						gRes /= denom;
						bRes /= denom;
					}
					else
					{
						rRes = rst[y][x].r;
						gRes = rst[y][x].g;
						bRes = rst[y][x].b;
					}
					
					
				//	cout<<"r g b Res"<<rRes<<" "<<gRes<<" "<<bRes<<endl;
					//-- User constraints updates
					
					//check for constant-reflectance
					Vec4b albedoVal(sim_alb[y][x].r, sim_alb[y][x].g, sim_alb[y][x].b, sim_alb[y][x].a);
				//	scribVal = scribbleSimAlbedo.pixel(x,y);
					if(albedoVal[3] != 0)
					{
						//constant-reflectance pixel
						//get the scribble id
					//	int id = albedoNumColors.indexOf(QColor(scribVal));
						vector<Vec3b>::const_iterator idd = std::find(albedoNumColors.begin(), albedoNumColors.end(), Vec3b(albedoVal[0], albedoVal[1], albedoVal[2]));
						if (idd == albedoNumColors.end())
						{
							cout<<"id error"<<endl;
							int id = idd - albedoNumColors.begin();	
							cout<<"id = "<<id<<endl;
							cout<<" "<<albedoVal[0]<<" "<<albedoVal[1]<<" "<<albedoVal[2]<<endl;
							return;
						}						

						int id = idd - albedoNumColors.begin();		
					
						//get the image value for this pixel
					//	imgVal = imgIn.pixel(x,y);
						Vec3f imgVal(img_in[y][x].r, img_in[y][x].g, img_in[y][x].b);

						//illumination = I(p) * sum(S)/sum(I)
						//if one channel is too small, take another one
						//red
						if(imgVal[0]>EPSILON_PIXEL_VAL && albedoAvgColors[id][0]>EPSILON_PIXEL_VAL)
						{
						//	S(p) = I(p)*sum(S)/sum(I)
							rRes = lambdaCstr*rRes + (1.0-lambdaCstr)*imgVal[0]*albedoAvgColors[id][0];
						}
						else
						{
							if(imgVal[1] > EPSILON_PIXEL_VAL && albedoAvgColors[id][1] > EPSILON_PIXEL_VAL)
							{
								rRes = lambdaCstr*gRes + (1.0-lambdaCstr)*imgVal[1]*albedoAvgColors[id][1];
							}
							else
							{
								rRes = lambdaCstr*bRes + (1.0-lambdaCstr)*imgVal[2]*albedoAvgColors[id][2];
							}
						}
						//green
						if(imgVal[1]>EPSILON_PIXEL_VAL && albedoAvgColors[id][1]>EPSILON_PIXEL_VAL)
						{
							gRes = lambdaCstr*gRes + (1.0-lambdaCstr)*imgVal[1]*albedoAvgColors[id][1];
						}
						else
						{
							gRes = rRes;
						}
						//blue
						if(imgVal[2]>EPSILON_PIXEL_VAL && albedoAvgColors[id][2]>EPSILON_PIXEL_VAL)
						{
							bRes = lambdaCstr*bRes + (1.0-lambdaCstr) * imgVal[2] * albedoAvgColors[id][2];
						}
						else
						{
							bRes = rRes;
						}
					}

					//constant shading 
					Vec4b shaVal(sim_sha[y][x].r, sim_sha[y][x].g, sim_sha[y][x].b, sim_sha[y][x].a);
					if (shaVal[3] != 0)
					{
						//check for constant-illumination
					//	scribVal = scribbleSimShading[deepness-1].pixel(x,y);
						vector<Vec3b>::const_iterator idd = std::find(shadingNumColors.begin(), shadingNumColors.end(), Vec3b(shaVal[0], shaVal[1], shaVal[2]));
						if (idd == shadingNumColors.end())
						{
							cout<<"shading id error"<<endl;
							return;
						}						

						int id = idd - shadingNumColors.begin();	
						rRes = lambdaCstr*rRes + (1.0-lambdaCstr)*shadingAvgColors[id][0];
						gRes = lambdaCstr*gRes + (1.0-lambdaCstr)*shadingAvgColors[id][1];
						bRes = lambdaCstr*bRes + (1.0-lambdaCstr)*shadingAvgColors[id][2];
// 						if(qAlpha(scribVal)>1){//constant-illumination pixel
// 							//get the scribble id
// 							int id = shadingNumColors.indexOf(QColor(scribVal));
// 							
// 						}

					}				

				}
				if(rRes<EPSILON_PIXEL_VAL)
				{
					rRes = EPSILON_PIXEL_VAL;
				}
				if(gRes<EPSILON_PIXEL_VAL)
				{
					gRes = EPSILON_PIXEL_VAL;
				}
				if(bRes<EPSILON_PIXEL_VAL)
				{
					bRes = EPSILON_PIXEL_VAL;
				}
			//	result.setPixel(x,y,Color(rRes,gRes,bRes));
				rst[y][x].r = rRes;
				rst[y][x].g = gRes;
				rst[y][x].b = bRes;
			}
		}
	}

	//show result
	result.convertTo(result, CV_8U, 255.0); 
	imshow("shading", result);
	imwrite("flower_shading.png", result);
	cvWaitKey(0);

}


#if 0
int main()
{
	//computer L matrix
 	string orgFile = string("flower\\pink_flower.tif");
 	Mat org = imread(orgFile, -1);
 	if (org.empty())
 		return -1;
 	if (org.depth() == CV_8U)
 	{
 		org.convertTo(org, CV_32F, 255.0/(1<<7));
 	}
 	else if (org.depth() == CV_16U)
 	{
 		org.convertTo(org, CV_32F, 255.0/(1<<15));
 	}
 	else
 	{
 		cout<<"data type error"<<endl;
 		return -1;
 	}	
	
 	buildLMatrix(org);
	int w = org.cols;
	int h = org.rows;

	double tt = clock();

	Mat LMatrix;
	FileStorage fs("LMatrix.xml", FileStorage::READ);	
	fs["LMatrix"]>>LMatrix;	

	cout<<"cost time = "<<clock()-tt<<endl;

	if (LMatrix.empty())
	{
		return -1;
	}
	else
	{
		cout<<"read LMatrix"<<endl;
	}
	fs.release();

	string absoluteShading = string("flower\\scribbleAbsolute.png");
	string simAlbedo = string("flower\\scribbleSimAlbedo.png");
	string simShading = string("flower\\scribbleSimShading.png");

	Mat absSha = imread(absoluteShading, -1);
	Mat simAlb = imread(simAlbedo, -1);
	Mat simSha = imread(simShading, -1);

	assert(!absSha.empty());
	assert(!simAlb.empty());
	assert(!simSha.empty());

	cout<<"channel = "<<absSha.channels()<<endl;
	cout<<"channel = "<<simAlb.channels()<<endl;
	cout<<"channel = "<<simSha.channels()<<endl;

	imshow("abs sha", absSha);
	imshow("sim alb", simAlb);
	imshow("sim sha", simSha);

	//test absSha
// 	ArgbU8Image sha_abs(&absSha);
// 	cout<<"test shading abstract"<<endl;
// 	for (int y = 0; y<h; ++y)
// 	{
// 		for (int x = 0; x<w; ++x)
// 		{
// 			if (sha_abs[y][x].a != 0)
// 			{
// 				cout<<(int)sha_abs[y][x].r<<endl;
// 			}
// 		}
// 	}
	
	
	vector<Vec3b> simAlbVec;
	vector<Vec3b> simShaVec;

	Argb8UImage sim_alb(&simAlb);
	Argb8UImage sim_sha(&simSha);
	for (int y = 0; y<h; ++y)
	{
		for (int x = 0; x<w; ++x)
		{
			if (sim_alb[y][x].a != 0)
			{
				Vec3b alb_pix(sim_alb[y][x].r, sim_alb[y][x].g, sim_alb[y][x].b);
				if (std::find(simAlbVec.begin(), simAlbVec.end(), alb_pix) == simAlbVec.end())
				{
					simAlbVec.push_back(alb_pix);
				}

				
			}

			if (sim_sha[y][x].a != 0)
			{
				Vec3b sha_pix(sim_sha[y][x].r, sim_sha[y][x].g, sim_sha[y][x].b);
				if (std::find(simShaVec.begin(), simShaVec.end(), sha_pix) == simShaVec.end())
				{
					simShaVec.push_back(sha_pix);
				}
			}
		}
	}
	for (int i = 0; i<simAlbVec.size(); ++i)
	{
		cout<<(int)simAlbVec[i][0]<<" "<<(int)simAlbVec[i][1]<<" "<<(int)simAlbVec[i][2]<<" "<<endl;
	}
	cout<<endl;

	for (int i = 0; i<simShaVec.size(); ++i)
	{
		cout<<(int)simShaVec[i][0]<<" "<<(int)simShaVec[i][1]<<" "<<(int)simShaVec[i][2]<<" "<<endl;
	}

	solveA(org, LMatrix, absSha, simAlb, simSha, simAlbVec, simShaVec);	

	cvWaitKey(0);

//	buildLMatrix

	return 1;
}
#endif

#if 0
int main()
{
	//store data in xml
	FileStorage fs("test.xml", FileStorage::WRITE);

	string orgFile = string("flower\\pink_flower.tif");
	Mat org = imread(orgFile, -1);

	fs<<"frameCount"<<5;
	time_t rawtime;
	time(&rawtime);
	fs <<"calibrationDate"<< asctime(localtime(&rawtime));
	Mat cameraMatrix = (Mat_<double>(3, 3) <<1000, 0, 320, 0, 1000, 240, 0, 0, 1);
	Mat distCoeffs = (Mat_<double>(5, 1) << 0.1, 0.01, -0.001, 0, 0);
	fs<<"cameraMatrix"<<cameraMatrix<<"distCoeffs"<<distCoeffs;
	fs<<"flower"<<org;
	fs.release();

	//read data from xml

	FileStorage fs2("test.xml", FileStorage::READ);

	Mat orgImage;
	fs2["flower2"]>>orgImage;
	if (!orgImage.empty())
	{
		imshow("flower", orgImage);
	}
	cvWaitKey(0);
	return 1;
}
#endif

#if 0
int main()
{
	string orgFile = string("flower\\pink_flower.tif");
	Mat org = imread(orgFile, -1);
	if (org.empty())
	{
		printf("can't load .tif file.");
		return -1;
	}
	if (org.type() == IPL_DEPTH_8U)
	{
		printf("IPL_DEPTH_8U");
	}
	printf("depth = %d\n", org.depth());
	printf("channel = %d\n", org.channels());

	imshow("flower", org);

	int h = org.rows;
	int w = org.cols;


	RgbU16Image org_img(&org);
	for (int y = 0; y<h; ++y)
	{
		for (int x =0 ; x<w; ++x)
		{
		//	int r = org_img[y][x].r;
		//	cout<<"r = "<<r<<endl;
 			org_img[y][x].r = 0;
 			org_img[y][x].g = 0;
 			org_img[y][x].b = 1<<15;	
		}
	}

	imshow("flower modified", org);

	cvWaitKey(0);

	//L matrix	
}
#endif